#include "bsp_can_motor.h"
#include "bsp_can_core.h"
#include <string.h>
#include <math.h>

// 解析和更新电机反馈数据
static void _update_motor_feedback(uint8_t motor_idx, bsp_can_msg_t *msg);
// 检查电机连接状态
static void _check_motor_connection(void);

// 电机配置
static const uint16_t MOTOR_CAN_IDS[MOTOR_COUNT] = {
    MOTOR_A_CAN_ID, MOTOR_B_CAN_ID, MOTOR_C_CAN_ID, MOTOR_D_CAN_ID
};

// 电机反馈数据
static motor_feedback_t motor_feedbacks[MOTOR_COUNT];
static motor_config_t motor_configs[MOTOR_COUNT];

// 默认电机配置
static const motor_config_t DEFAULT_MOTOR_CONFIG = {
    .reduction_ratio = 1.0f,
    .max_current = 20.0f,
    .max_speed = 8000.0f,
    .current_to_raw = 1000
};

// 上一次角度值（用于计算累计角度）
static int16_t last_angle_raw[MOTOR_COUNT] = {0};

/**
 * @brief 初始化电机模块
 */
uint8_t bsp_can_motor_init(void)
{
    // 初始化电机数据
    memset(motor_feedbacks, 0, sizeof(motor_feedbacks));
    memset(last_angle_raw, 0, sizeof(last_angle_raw));
    
    for (int i = 0; i < MOTOR_COUNT; i++) {
        motor_feedbacks[i].connected = false;
        motor_configs[i] = DEFAULT_MOTOR_CONFIG;
    }
    
    // 配置电机过滤器
    bsp_can_add_filter(MOTOR_FB_MIN_ID, 0x7F0);
    
    return 0;
}

/**
 * @brief 发送单个电机电流指令
 */
uint8_t bsp_can_send_motor_current(motor_id_t motor, int16_t current)
{
    if (motor >= MOTOR_COUNT) return 1;
    
    uint8_t data[8] = {0};
    int16_t limited_current = current;
    
    // 电流限制
    float max_current_raw = motor_configs[motor].max_current * motor_configs[motor].current_to_raw;
    if (limited_current > max_current_raw) limited_current = max_current_raw;
    if (limited_current < -max_current_raw) limited_current = -max_current_raw;
    
    // 根据电机ID设置数据位置
    uint8_t offset = motor * 2;
    data[offset] = limited_current >> 8;
    data[offset + 1] = limited_current & 0xFF;
    
    return bsp_can_send(MOTOR_CTRL_ALL_ID, data, 8);
}

/**
 * @brief 发送所有电机电流指令
 */
uint8_t bsp_can_send_all_motors_current(int16_t current_a, int16_t current_b, int16_t current_c, int16_t current_d)
{
    uint8_t data[8] = {
        current_a >> 8, current_a & 0xFF,
        current_b >> 8, current_b & 0xFF, 
        current_c >> 8, current_c & 0xFF,
        current_d >> 8, current_d & 0xFF
    };
    
    return bsp_can_send(MOTOR_CTRL_ALL_ID, data, 8);
}

/**
 * @brief 麦轮电流控制
 */
uint8_t bsp_can_send_mecanum_current(int16_t vx, int16_t vy, int16_t vz)
{
    // 麦轮运动学公式
    int16_t wheel_a = -vx + vy + vz;
    int16_t wheel_b = vx + vy + vz; 
    int16_t wheel_c = vx + vy - vz;
    int16_t wheel_d = -vx + vy - vz;
    
    return bsp_can_send_all_motors_current(wheel_a, wheel_b, wheel_c, wheel_d);
}

/**
 * @brief 麦轮速度控制
 */
uint8_t bsp_can_send_mecanum_velocity(float vx, float vy, float vz)
{
    // 将速度转换为电流指令（需要外部PID）
    int16_t wheel_a = bsp_can_rpm_to_raw_velocity(-vx + vy + vz);
    int16_t wheel_b = bsp_can_rpm_to_raw_velocity(vx + vy + vz);
    int16_t wheel_c = bsp_can_rpm_to_raw_velocity(vx + vy - vz);
    int16_t wheel_d = bsp_can_rpm_to_raw_velocity(-vx + vy - vz);
    
    return bsp_can_send_all_motors_current(wheel_a, wheel_b, wheel_c, wheel_d);
}

/**
 * @brief 处理电机数据
 */
void bsp_can_motor_process(void)
{
    // 处理接收到的CAN消息
    bsp_can_msg_t msg;
    while (bsp_can_receive(&msg) == 0) {
        if (msg.id >= MOTOR_FB_MIN_ID && msg.id <= MOTOR_FB_MAX_ID) {
            for (int i = 0; i < MOTOR_COUNT; i++) {
                if (msg.id == MOTOR_CAN_IDS[i]) {
                    _update_motor_feedback(i, &msg);
                    break;
                }
            }
        }
    }
    
    // 检查连接状态
    _check_motor_connection();
}

/**
 * @brief 更新电机反馈数据
 */
static void _update_motor_feedback(uint8_t motor_idx, bsp_can_msg_t *msg)
{
    motor_feedback_t *fb = &motor_feedbacks[motor_idx];
    motor_config_t *config = &motor_configs[motor_idx];
    
    // 解析原始数据
    fb->angle_raw = (msg->data[0] << 8) | msg->data[1];
    fb->velocity_raw = (msg->data[2] << 8) | msg->data[3];
    fb->current_raw = (msg->data[4] << 8) | msg->data[5];
    
    // 转换为工程单位
    fb->angle_deg = bsp_can_raw_angle_to_deg(fb->angle_raw);
    fb->velocity_rpm = bsp_can_raw_velocity_to_rpm(fb->velocity_raw) / config->reduction_ratio;
    fb->current_a = bsp_can_raw_current_to_amp(fb->current_raw);
    
    // 计算累计角度（处理溢出）
    int16_t delta = fb->angle_raw - last_angle_raw[motor_idx];
    if (delta > 8192) delta -= 16384;
    else if (delta < -8192) delta += 16384;
    
    fb->total_angle += delta;
    fb->total_angle_deg = (float)fb->total_angle * 360.0f / 16384.0f / config->reduction_ratio;
    last_angle_raw[motor_idx] = fb->angle_raw;
    
    // 更新状态
    fb->last_update = msg->timestamp;
    fb->connected = true;
    fb->msg_count++;
}

/**
 * @brief 检查电机连接状态
 */
static void _check_motor_connection(void)
{
    uint32_t now = HAL_GetTick();
    for (int i = 0; i < MOTOR_COUNT; i++) {
        if (motor_feedbacks[i].connected && (now - motor_feedbacks[i].last_update > 100)) {
            motor_feedbacks[i].connected = false;
        }
    }
}

/**
 * @brief 获取电机反馈数据指针（用于PID）
 */
motor_feedback_t* bsp_can_get_motor_feedback_ptr(motor_id_t motor)
{
    return (motor < MOTOR_COUNT) ? &motor_feedbacks[motor] : NULL;
}

/**
 * @brief 获取电机反馈数据
 */
motor_feedback_t bsp_can_get_motor_feedback(motor_id_t motor)
{
    return (motor < MOTOR_COUNT) ? motor_feedbacks[motor] : (motor_feedback_t){0};
}

/**
 * @brief 检查电机是否连接
 */
bool bsp_can_is_motor_connected(motor_id_t motor)
{
    return (motor < MOTOR_COUNT) ? motor_feedbacks[motor].connected : false;
}

/**
 * @brief 获取连接电机数量
 */
uint8_t bsp_can_get_connected_motors(void)
{
    uint8_t count = 0;
    for (int i = 0; i < MOTOR_COUNT; i++) {
        if (motor_feedbacks[i].connected) count++;
    }
    return count;
}

/**
 * @brief 数据转换函数
 */
float bsp_can_raw_angle_to_deg(int16_t raw_angle) {
    return (float)raw_angle * 360.0f / 16384.0f;
}

float bsp_can_raw_velocity_to_rpm(int16_t raw_velocity) {
    return (float)raw_velocity * 1.0f;
}

float bsp_can_raw_current_to_amp(int16_t raw_current) {
    return (float)raw_current / 16384.0f * 20.0f;
}

int16_t bsp_can_amp_to_raw_current(float current_amp) {
    return (int16_t)(current_amp / 20.0f * 16384.0f);
}

int16_t bsp_can_rpm_to_raw_velocity(float rpm) {
    return (int16_t)(rpm * 1.0f);
}

/**
 * @brief 设置电机配置
 */
void bsp_can_set_motor_config(motor_id_t motor, motor_config_t config)
{
    if (motor < MOTOR_COUNT) {
        motor_configs[motor] = config;
    }
}

/**
 * @brief 获取电机配置
 */
motor_config_t bsp_can_get_motor_config(motor_id_t motor)
{
    return (motor < MOTOR_COUNT) ? motor_configs[motor] : DEFAULT_MOTOR_CONFIG;
}

/**
 * @brief 探测电机
 */
uint8_t bsp_can_detect_motors(uint16_t *motor_ids, uint8_t max_motors, uint32_t timeout_ms)
{
    if (!motor_ids) return 0;
    
    uint32_t start_time = HAL_GetTick();
    uint8_t found_count = 0;
    
    // 临时清空接收队列
    bsp_can_clear_rx_queue();
    
    while ((HAL_GetTick() - start_time) < timeout_ms && found_count < max_motors) {
        bsp_can_motor_process();
        
        // 检查接收到的电机反馈
        for (int i = 0; i < MOTOR_COUNT && found_count < max_motors; i++) {
            if (motor_feedbacks[i].connected && 
                (HAL_GetTick() - motor_feedbacks[i].last_update < 50)) {
                
                // 检查是否已经记录
                bool already_found = false;
                for (int j = 0; j < found_count; j++) {
                    if (motor_ids[j] == MOTOR_CAN_IDS[i]) {
                        already_found = true;
                        break;
                    }
                }
                
                if (!already_found) {
                    motor_ids[found_count++] = MOTOR_CAN_IDS[i];
                }
            }
        }
        
        HAL_Delay(10);
    }
    
    return found_count;
}